#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "wallyingLib.h"

#define BLE_CONNECTION_CHANNEL_NUM (37)

uint16_t Connection_Event_Counter = 522;//0;



static uint8_t lastUnmappedChannel = 30;//0

static uint16_t connEventCounter = 0;

uint8_t hopIncrement = 8;//0;
uint8_t channelMap[5] =
//{0xFF, 0xFF, 0xFF, 0xFF, 0x1F};
{0x01, 0xbe, 0xf8, 0x01, 0x00};

static uint8_t unmappedChannel = 0;
static uint8_t numUsedChannels = 0;

static uint8_t remappingIndex = 0;
static uint8_t remappingTable[BLE_CONNECTION_CHANNEL_NUM] = {0x00};


uint8_t dataChannelIndex = 0;


int main(int argc, char *argv[])
{
    printf("Hello World!\r\n");


	printf("lastUnmappedChannel = %d\r\n", lastUnmappedChannel);


	connEventCounter = Connection_Event_Counter;
	printf("connEventCounter = %d\r\n", connEventCounter);


	numUsedChannels = bits_count(channelMap, BLE_CONNECTION_CHANNEL_NUM);
	printf("numUsedChannels = %d\r\n", numUsedChannels);


	memset(remappingTable, 0, sizeof(remappingTable));
	for (int i = 0, j = 0; i < BLE_CONNECTION_CHANNEL_NUM; i++) {
		if (bit_get(channelMap, i)) {
			remappingTable[j] = i;
			j++;
		}
	}

	while (1)
	{
		unmappedChannel = (lastUnmappedChannel + hopIncrement) % 37;

		if (bit_get(channelMap, unmappedChannel))
		{
			dataChannelIndex = unmappedChannel;
		}
		else
		{
			remappingIndex = unmappedChannel % numUsedChannels;
			dataChannelIndex = remappingTable[remappingIndex];
		}

		printf("connEventCounter: %d --- dataChannelIndex: %d (remappingIndex: %d)\r\n", connEventCounter, dataChannelIndex, remappingIndex);
		lastUnmappedChannel = unmappedChannel;

		connEventCounter++;

		system("pause");
	}


    system("pause");
    return 0;
}
